#include "can_driver.h"

// 声明处理函数
void process_canard_rx_frame(const CanardCANFrame* frame);

// CAN Filter Configuration - Match all IDs
uint8_t CAN_Filter_Config(void)
{
  CAN_FilterTypeDef canfilter;

  // Configure filter 0 to match all standard and extended IDs
  canfilter.FilterBank = 0;                      // Use filter 0
  canfilter.FilterMode = CAN_FILTERMODE_IDMASK;  // Mask mode
  canfilter.FilterScale = CAN_FILTERSCALE_32BIT; // 32-bit mode
  canfilter.FilterIdHigh = 0x0000;               // ID high 16 bits
  canfilter.FilterIdLow = 0x0000;                // ID low 16 bits  
  canfilter.FilterMaskIdHigh = 0x0000;           // Mask high 16 bits - all 0 means don't check
  canfilter.FilterMaskIdLow = 0x0000;            // Mask low 16 bits - all 0 means don't check
  canfilter.FilterFIFOAssignment = CAN_RX_FIFO0; // Assign to FIFO0
  canfilter.FilterActivation = ENABLE;           // Enable filter
  canfilter.SlaveStartFilterBank = 14;           // Number of slave filter banks

  if (HAL_CAN_ConfigFilter(&hcan1, &canfilter) != HAL_OK)
  {
    return 1; // Configuration failed
  }
  
  return 0; // Configuration successful
}

// CAN message send function
uint8_t CAN_Send_Msg(uint32_t id, uint8_t* data, uint8_t len, uint8_t ext_id)
{
  CAN_TxHeaderTypeDef tx_header;
  uint32_t tx_mailbox;
  uint8_t tx_data[8];
  
  if (len > 8) len = 8; // CAN frame max 8 bytes
  
  // Fill data
  for (int i = 0; i < len; i++) {
    tx_data[i] = data[i];
  }
  
  // Configure transmit header
  if (ext_id) {
    tx_header.ExtId = id;                    // Extended ID
    tx_header.IDE = CAN_ID_EXT;              // Extended ID mode
  } else {
    tx_header.StdId = id;                    // Standard ID
    tx_header.IDE = CAN_ID_STD;              // Standard ID mode
  }
  
  tx_header.RTR = CAN_RTR_DATA;              // Data frame
  tx_header.DLC = len;                       // Data length
  tx_header.TransmitGlobalTime = DISABLE;
  
  // Start transmission
  if (HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data, &tx_mailbox) != HAL_OK) {
    return 1; // Transmission failed
  }
  
  return 0; // Transmission successful
}

// FIFO0 receive interrupt callback function
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan){
    CAN_RxHeaderTypeDef rxHeader;
    uint8_t rxData[8];
    
    if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rxHeader, rxData) == HAL_OK)
    {
        if (rxHeader.IDE == CAN_ID_EXT) {
            uint32_t raw_can_id = rxHeader.ExtId;
            
            // 去除 CAN 扩展帧标志位，得到实际的 DroneCAN ID
            uint32_t dronecan_id = raw_can_id & 0x1FFFFFFF; // 清除 EFF, RTR, ERR 标志
            
            uint8_t transfer_type = (dronecan_id >> 24) & 0xF;
            uint16_t data_type_id = (dronecan_id >> 8) & 0xFFFF;
            uint8_t source_node_id = dronecan_id & 0xFF;
            
            printf("RX - Raw CAN ID: 0x%08X, DroneCAN ID: 0x%08X\n", raw_can_id, dronecan_id);
            printf("     TransferType: %d, DataTypeID: %d, SourceNode: %d, Data: ",
                   transfer_type, data_type_id, source_node_id);
            
            for(int i = 0; i < rxHeader.DLC; i++) {
                printf("%02X ", rxData[i]);
            }
            printf("\n");
            
            if (data_type_id == UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ID) {
                printf("*** This is a CircuitStatus message! ***\n");
            } else if (data_type_id == 2796) { // NodeStatus
                printf("This is a NodeStatus message (heartbeat)\n");
            }
            
            // 处理接收到的帧
            CanardCANFrame received_frame;
            received_frame.id = raw_can_id; // 使用原始 CAN ID
            received_frame.data_len = rxHeader.DLC;
            memcpy(received_frame.data, rxData, rxHeader.DLC);
            received_frame.iface_id = 0;
            
            process_canard_rx_frame(&received_frame);
        }
    }
}
